#include "WPILib.h"
#include "input.h"
#include "Constants.h"
#include "DriveTrain.h"
#include "HenningCamera.h"
#include "Turret.h"
#include "Autonomous.h"

#include "vxWorks.h" 
#include "AxisCamera.h" 
#include "BaeUtilities.h"
#include "FrcError.h"
#include "PCVideoServer.h"

class BuiltinDefaultCode : public IterativeRobot
{
	InputHandler *m_input;
	DriveTrain *m_drive;
	Camera *m_camera;
	Turret *m_turret;
	Autonomous *m_auton;
	DigitalModule *m_digMod2;
	int m_autonTimer;
		
public:
/**
 * Constructor for this "BuiltinDefaultCode" Class.
 */
	BuiltinDefaultCode(void)	{
		printf("BuiltinDefaultCode Constructor Started\n");
		m_digMod2 = DigitalModule::GetInstance(5);
		
		m_input = new InputHandler();
		m_drive = new DriveTrain(m_input);

		TrackingThreshold td1, td2;
		sprintf (td1.name, "PINK");
		td1.hue.minValue = 220;   
		td1.hue.maxValue = 255;  
		td1.saturation.minValue = 75;   
		td1.saturation.maxValue = 255;      
		td1.luminance.minValue = 85;  
		td1.luminance.maxValue = 255;
		sprintf (td2.name, "GREEN");
		td2.hue.minValue = 55;   
		td2.hue.maxValue = 125;  
		td2.saturation.minValue = 58;   
		td2.saturation.maxValue = 255;    
		td2.luminance.minValue = 92;  
		td2.luminance.maxValue = 255;
		CameraLib::MyOrder myOrder;
		if (m_input->InputHandler::GetTargetOrientation()) {
			myOrder = CameraLib::GREENPINK;
			printf("GreenPink\n");
		} else {
			myOrder = CameraLib::PINKGREEN;
			printf("PinkGreen\n");
		}
		m_camera = new Camera(td1, td2, myOrder, HORIZONTAL_SERVO, VERTICAL_SERVO, m_input);

		m_turret = new Turret(m_input, m_camera);
		m_auton = new Autonomous(m_input, m_drive, m_camera);
		m_autonTimer = 0;
		
		GetWatchdog().SetExpiration(0.5);
		GetWatchdog().SetEnabled(false);
		
		printf("BuiltinDefaultCode Constructor Completed\n");
	}
	
	/********************************** Periodic Routines *************************************/
	void DisabledPeriodic(void)  {
		// feed the user watchdog at every period when disabled
		GetWatchdog().Feed();
	}
	
	void AutonomousPeriodic(void) {
		// feed the user watchdog at every period when in autonomous
		GetWatchdog().Feed();
		m_autonTimer++;
		
		//m_auton->AutoAimTurret();		
		//m_turret->AimWithCamera();

		m_input->Run();
		
		int mode = m_input->GetAutonMode();
		printf("mode:%d\n", mode);
		
		if (mode == 0) {
			if (m_autonTimer < 1000) m_drive->Tank(-1, -1);
		} else if (mode == 1) {
			if (m_autonTimer < 1*200) m_drive->Tank(-.75, -1);
			else if (m_autonTimer < 1.5*200) m_drive->Tank(1, -1);
			else m_drive->Tank(-1, -1);
		} else if (mode == 2) {
		} else if (mode == 3) {}
		
		/*
		 * Vdrive simple auton
		if (m_autonTimer < 3*200) {
			m_drive->SteerBack(-30);
			m_drive->SteerFront(-30);
			m_drive->SetSpeed(-1);
		} else if (m_autonTimer < 6*200) {
			m_drive->SteerBack(-15);
			m_drive->SteerFront(-15);
			m_drive->SetSpeed(-1);
		} else {
			m_drive->SteerBack(0);
			m_drive->SteerFront(0);
			m_drive->SetSpeed(-1);
		}
		*/
	}

	
	void TeleopPeriodic(void) {
		// feed the user watchdog at every period when in autonomous
		GetWatchdog().Feed();

		m_input->Run();
		
		//if (m_input->GetPureAutonomous()) {
		//	m_auton->PureAutonomousPeriodic();
		//} else {
			m_drive->WheelDrive();
			
			/*
			if (m_input->GetAutoTrack()) {
				m_auton->AutoAimTurret();
				m_turret->AimWithCamera();
			} else {
				printf("target=%e", m_input->GetAimTurret());
				m_turret->Aim(m_input->GetAimTurret());
				//m_turret->AimCameraWithTurret();
			}
			
			if (m_input->GetAutoFire()) {
				m_auton->AutoFire();
			} else {
				if (m_input->GetFire()) m_turret->FireBall();
				else m_turret->UnFireBall();
			}*/
			
			//m_turret->Aim(m_input->GetAimTurret());
			//m_turret->Aim(m_input->GetDriveSpeed());
			//printf("pot_val:%f\n", m_input->GetTurretAngle());

			if (m_input->GetFire()) m_turret->FireBall();
			else m_turret->UnFireBall();
			
			m_turret->MoveBelts(m_input->GetMoveBelts());
		//}
		//if (m_input->m_leftStickButtonState[1]) m_turret->FireBall();
		//else m_turret->UnFireBall();
		} // TeleopPeriodic(void)
};

START_ROBOT_CLASS(BuiltinDefaultCode);
